'''
PID使用说明：
    输入参数：当前的编码器值，目标速度对应的编码器值
    输出参数：此时应当的给电机的pwm值
'''
class PID:
    def __init__(self, pwm_range, kp, ki, kd):
        pwm = 0
        err = 0
        last_err = 0
        self.pwm_range = pwm_range
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.pwm = pwm
        self.err = err
        self.last_err = last_err
        
    def incremental_pid(self, now, target):
        '''
        函数功能：增量PI控制器
        入口参数：当前的编码器值，目标速度对应的编码器值
        返回值  ：电机PWM
        根据增量式离散PID公式：
        pwm += Kp[e(k) - e(k-1)] + Ki*e(k) + Kd[e(k) - 2e(k-1) + e(k-2)]
        e(k)代表本次偏差；
        e(k-1)代表上一次的偏差；    以此类推
        pwm代表增量输出。
        '''
        self.err = target - now
        # print("pre-pwm:", self.pwm)
        self.pwm = self.pwm + self.kp*(self.err - self.last_err) + self.ki*self.err + self.kd*(self.err - self.last_err)
        if (self.pwm >= self.pwm_range):
            self.pwm = self.pwm_range
        if (self.pwm <= -self.pwm_range):
            self.pwm = -self.pwm_range
        self.last_err = self.err
        # print("err:", self.err)
        # print("last err:", self.last_err)
        # print("pwm:", self.pwm)
        abs_pwm = abs(self.pwm)
        return abs_pwm